﻿using System.Numerics;

namespace GeometryLib
{
    public class PlaneCalculator
    {
        private const double epsilon = 1e-1;

        public static bool FourPointPlane(Point3D p1, Point3D p2, Point3D p3,Point3D pt4)
        {
            var plane = CalculatePlane(p1, p2, p3);

            var distance = DistanceToPlane(pt4, plane);
#if DEBUG
            Console.WriteLine($"{distance}");
#endif
            if (Math.Abs( distance) < epsilon) return true;
            return false;
        }


        // 三点计算平面方程 
        private static (double A, double B, double C, double D) CalculatePlane(Point3D p1, Point3D p2, Point3D p3)
        {
            // 计算两个向量 
            var v1 = new Point3D(p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z);
            var v2 = new Point3D(p3.X - p1.X, p3.Y - p1.Y, p3.Z - p1.Z);

            // 叉乘求法向量 
            double A = v1.Y * v2.Z - v1.Z * v2.Y;
            double B = v1.Z * v2.X - v1.X * v2.Z;
            double C = v1.X * v2.Y - v1.Y * v2.X;

            // 计算D参数 
            double D = -(A * p1.X + B * p1.Y + C * p1.Z);

            return (A, B, C, D);
        }

        // 点到平面距离公式 
        private static double DistanceToPlane(Point3D point, (double A, double B, double C, double D) plane)
        {
            return Math.Abs(plane.A * point.X + plane.B * point.Y + plane.C * point.Z + plane.D)
                   / Math.Sqrt(plane.A * plane.A + plane.B * plane.B + plane.C * plane.C);
        }

        /// <summary>
        /// 四点是否共线
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="c"></param>
        /// <param name="d"></param>
        /// <returns>true 共线；false 不共线</returns>
        public static bool ArePointsCollinear(Point3D a, Point3D b, Point3D c, Point3D d)
        {
            // 检查前三个点是否共线 
            Vector3 ab = new Vector3((float) (b.X - a.X), (float)(b.Y - a.Y), (float)(b.Z - a.Z));

            Vector3 ac = new Vector3((float)(c.X - a.X), (float)(c.Y - a.Y), (float)(c.Z - a.Z));
            Vector3 crossABC = Vector3.Cross(ab, ac);
            if (Vector3.Distance(crossABC,Vector3.Zero) > 1e-6) // 允许误差范围 
            {
                return false; // 前三点不共线，四点不可能共线 
            }

            // 检查第四点是否与A、B共线 
            Vector3 ad = new Vector3((float)(d.X - a.X), (float)(d.Y - a.Y), (float)(d.Z - a.Z));
            Vector3 crossABD = Vector3.Cross(ab, ad);

            var fdistance = Vector3.Distance(crossABD, Vector3.Zero);
#if DEBUG
            Console.WriteLine($"第四点：{fdistance}");
#endif
            return  fdistance <= 1e-6;
        }

    }
}
